/*
 * Copyright (c) from 2000 to 2009
 * 
 * Network and System Laboratory 
 * Department of Computer Science 
 * College of Computer Science
 * National Chiao Tung University, Taiwan
 * All Rights Reserved.
 * 
 * This source code file is part of the NCTUns 6.0 network simulator.
 *
 * Permission to use, copy, modify, and distribute this software and
 * its documentation is hereby granted (excluding for commercial or
 * for-profit use), provided that both the copyright notice and this
 * permission notice appear in all copies of the software, derivative
 * works, or modified versions, and any portions thereof, and that
 * both notices appear in supporting documentation, and that credit
 * is given to National Chiao Tung University, Taiwan in all publications 
 * reporting on direct or indirect use of this code or its derivatives.
 *
 * National Chiao Tung University, Taiwan makes no representations 
 * about the suitability of this software for any purpose. It is provided 
 * "AS IS" without express or implied warranty.
 *
 * A Web site containing the latest NCTUns 6.0 network simulator software 
 * and its documentations is set up at http://NSL.csie.nctu.edu.tw/nctuns.html.
 *
 * Project Chief-Technology-Officer
 * 
 * Prof. Shie-Yuan Wang <shieyuan@csie.nctu.edu.tw>
 * National Chiao Tung University, Taiwan
 *
 * 09/01/2009
 */

#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <math.h> 
#include <string.h>

#include "math_fun.h"
#include "command_format.h"
#include "auto_vehicle_signal.h"

using namespace std;

// SE uses this function to read the traffic signals from text file which generated by GUI.
int read_signals(FILE *fptr, TrafficLightCenter &SignalList, vector<TrafficGroupInfo>&TrafficGroupInfoList)
{
	char line[256];
	double x, y, FacingDirection;
	int type;
	int numOfSignals = 0;
	int oldGID = 0;

	SignalList.MaxGroupID = -1;

	//signal temp;
	while(!feof(fptr)){
		line[0]='\0';
		fgets(line, 255, fptr);
		if(line[0]=='\0'||line[0]=='#')
			continue;
		if(sscanf(line, "%lf,%lf,%lf,%d", &x, &y, &FacingDirection, &type) != 4)
		{
			printf("[%s]: .sig File Format Error\n", __func__);
			return -1;
		}
		if(type == TRAFFIC_LIGHT)
		{
			int sigGID, light;

			fgets(line, 255, fptr);
			if(sscanf(line, "%d,%d", &sigGID, &light) != 2)
			{
				printf("[%s]: .sig File Format Error: wrong sigGID, light\n", __func__);
				return -1;
			}

			if(sigGID > MAX_NUM_SIG)
			{
				printf("[%s]: signal group is over than %d\n", __func__, MAX_NUM_SIG);
				return -1;
			}

			if(oldGID != sigGID)
			{
				// it's new sigGID
				oldGID = sigGID;
				numOfSignals = 0;
			}

			if(sigGID > SignalList.MaxGroupID)
				SignalList.MaxGroupID = sigGID;

			(SignalList.trafficLight[sigGID]).type = type;
			(SignalList.trafficLight[sigGID]).sigGID = sigGID;
			(SignalList.trafficLight[sigGID]).light[numOfSignals] = light;
			(SignalList.trafficLight[sigGID]).x[numOfSignals] = x;
			(SignalList.trafficLight[sigGID]).y[numOfSignals] = y;
			(SignalList.trafficLight[sigGID]).FacingDirection[numOfSignals] = FacingDirection;

			numOfSignals ++;
			(SignalList.trafficLight[sigGID]).numOfSignals = numOfSignals;

			//printf("x: %lf, y: %lf, FD: %lf, type: %d\n", x, y, FacingDirection, type);
			bool inListOrNot = false;
			for(int i = 0; i<(int)TrafficGroupInfoList.size(); i++)
			{
				if(sigGID == TrafficGroupInfoList[i].sigGID)
				{
					inListOrNot = true;
					TrafficGroupInfoList[i].numOfSigs++;
					break;
				}
			}
			if(inListOrNot == false){
				TrafficGroupInfo temp;
				temp.sigGID = sigGID;
				temp.numOfSigs = 1;
				TrafficGroupInfoList.push_back(temp);
			}
		}
		else if(type == SPEED_LIMITATION)
		{
			double HighestSpeed, LowestSpeed;
			fgets(line, 255, fptr);
			sscanf(line, "%lf,%lf", &LowestSpeed, &HighestSpeed);
		}
		else
		{
			printf("[%s]: .sig Error: no such type\n", __func__);
			return -1;
		}
	}
	return 0;
}

/* This function changes the traffic light into red, yellow, or green */
int setSignalLight(TrafficLightCenter &SignalList, int groupID, int signalIndex, int light)
{
	if((groupID > SignalList.MaxGroupID) || (groupID < 0) || (signalIndex < 0))
		return 0; // fail

	if((SignalList.trafficLight[groupID]).type != TRAFFIC_LIGHT)
		return 0; // fail

	SignalList.trafficLight[groupID].light[signalIndex] = light;
	return 1; // success
}

/* get the traffic lights based on the specified group ID. */
int getSignalsInTheSameGroup(TrafficLightCenter signalList, int groupID, agentClientGetSignalsInTheSameGroupReply *tempG)
{
	int result = 0; // fail
	int numOfSignals = 0;

	for(int i = 0; i <= signalList.MaxGroupID; ++i)
	{
		if(groupID == signalList.trafficLight[i].sigGID)
		{
			numOfSignals = signalList.trafficLight[i].numOfSignals;
			if(numOfSignals == 0)
				break; // fail

			tempG->sigGID = signalList.trafficLight[i].sigGID;
			tempG->signalType = signalList.trafficLight[i].type;
			tempG->numOfSigs = numOfSignals;

			for(int j = 0; j < numOfSignals; ++j)
			{
				tempG->light[j]           = signalList.trafficLight[i].light[j];
				tempG->x[j]               = signalList.trafficLight[i].x[j];
				tempG->y[j]               = signalList.trafficLight[i].y[j];
				tempG->facingDirection[j] = signalList.trafficLight[i].FacingDirection[j];
			}
			result = 1; // success
			break;
		}
	}

	return result;
}

/* This function return an unset group of traffic lights' GID to engine */
int getAnUnusedSignalGID(vector<TrafficGroupInfo>&TrafficGroupInfoList, int &GID, int &numOfSignals)
{
	if(!TrafficGroupInfoList.empty())
	{
		TrafficGroupInfo temp;
		temp = TrafficGroupInfoList.back(); // get the last element
		GID = temp.sigGID;
		numOfSignals = temp.numOfSigs;
		TrafficGroupInfoList.pop_back(); // pop out the last element
		return 1; // success
	}
	else 
		return 0; // fail
}

/*
 * Note:
 *    signal group ID is related to Node block ID.
 *    Therefore, we can get current node block ID in front of me by calling getFrontNID (in road.cc)
 *    and then use the node block ID to search the traffic light.
 *
 * Parameters:
 *    my_x, my_y:	My current position
 *    myDirection:	My current moving direction viewed on GUI screen
 *    distance:		My search range in meters
 *    sigGID:		signal group ID which need to search (one signal group controls one intersection)
 *    light:		return signal light which is facing me (the opposite direction of myDirection)
 *    TrafficLightPOS_x, TrafficLightPOS_y: Traffic light position
 *    signalIndex:	return the searched traffic light index
 *    			(There are 4 traffic light in one intersection (group) by default)
 */
int getTheNearestPrecedingTrafficLight(TrafficLightCenter signalList, double my_x, double my_y, double myDirection, double distance, int sigGID, int &light, double &TrafficLightPOS_x, double &TrafficLightPOS_y, int &signalIndex)
{
	int result = 0; // fail

	if((sigGID < 0) || (signalList.MaxGroupID == -1) || (myDirection < 0))
		return result; // fail

	/* Check if this car passes the traffic light.
	 * If not, no need to search again.
	 * Note:
	 *    signalIndex may be reused in the next time
	 */
	if((sigGID != -1) && (signalIndex != -1) && (light == signalList.trafficLight[sigGID].light[signalIndex]))
	{
		double tempAngle = fmod(atan2(TrafficLightPOS_y - my_y, TrafficLightPOS_x - my_x)/PI*180 + 360, 360);
		tempAngle = fmod(360 - tempAngle, 360);

		double angleDiff = fabs(tempAngle - myDirection);

		if(angleDiff > 180)
			angleDiff = 360 - angleDiff;

		if(angleDiff <= 90)
		{
			/*
			printf("DEBUG: light %d, sigPos (%lf, %lf), sigGID %d, sigIndex %d\n",
					light, TrafficLightPOS_x, TrafficLightPOS_y, sigGID, signalIndex);
			*/
			return 1; // still in front of the previous searched traffic light.
		}
	}

	double miniSigDistance = 999999;

	// search signal group to find out which traffic light is in front of me
	if(sigGID > signalList.MaxGroupID)
	{
		printf("%s Warning: no group ID %d\n", __func__, sigGID);
		return result; // fail
	}

	int numOfSigs = signalList.trafficLight[sigGID].numOfSignals;

	// search each traffic light in signal group
	for(int j = 0; j < numOfSigs; ++j)
	{
		double sigX, sigY, sigDirection, angle;
		double sigDistance;

		sigX = signalList.trafficLight[sigGID].x[j];
		sigY = signalList.trafficLight[sigGID].y[j];
		sigDirection = signalList.trafficLight[sigGID].FacingDirection[j];

		sigDistance = Distance_BetweenTwoNode(my_x, my_y, sigX, sigY);

		/* check if this traffic light is inside my search range */
		if(sigDistance > distance)
			continue;

		angle = fabs(sigDirection - myDirection);
		if(angle >= 180)
			angle = 360 - angle;

		/* check if this traffic light is facing to me */
		if(fabs(180 - angle) > 15)
			continue;

		// mathematical angle
		angle = atan2(sigY - my_y, sigX - my_x)/PI*180;

		// translate to GUI viewed direction
		angle = fmod(360 - fmod(angle + 360, 360), 360);

		double difference = fabs(angle - myDirection);

		if(difference > 180)
			difference = 360 - difference;

		if((difference < 90) && (sigDistance < miniSigDistance))
		{
			/* This traffic light is in front of me
			 * and also the closest to me.
			 */
			light             = signalList.trafficLight[sigGID].light[j];
			TrafficLightPOS_x = signalList.trafficLight[sigGID].x[j];
			TrafficLightPOS_y = signalList.trafficLight[sigGID].y[j];
			signalIndex       = j;

			miniSigDistance = sigDistance;

			result = 1; // success
		}
	}

	return result;
}

